// Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "forward_command_controller/forward_command_controller.hpp"
#include "shared_parameters/shared_params_interface.hpp"
#include <algorithm>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include <regex>

#include "rclcpp/logging.hpp"
#include "rclcpp/qos.hpp"

namespace forward_command_controller
{
ForwardCommandController::ForwardCommandController() : ForwardControllersBase() {}

void ForwardCommandController::declare_parameters()
{
  // param_listener_ = std::make_shared<ParamListener>(get_node());
  get_shared_parameters();
}

controller_interface::CallbackReturn ForwardCommandController::read_parameters()
{
  // if (!param_listener_)
  // {
  //   RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init");
  //   return controller_interface::CallbackReturn::ERROR;
  // }
  // params_ = param_listener_->get_params();

  
  std::string joints_name;
  get_node()->get_parameter_or("joints", joints_name, std::string(""));
  std::vector<std::string> joints = parse_names(joints_name);
  if (/*params_.*/joints.empty())
  {
    RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty");
    return controller_interface::CallbackReturn::ERROR;
  }

  std::string interface_name;
  get_node()->get_parameter_or("interface_name", interface_name, std::string(""));
  if (/*params_.*/interface_name.empty())
  {
    RCLCPP_ERROR(get_node()->get_logger(), "'interface_name' parameter was empty");
    return controller_interface::CallbackReturn::ERROR;
  }

  for (const auto & joint : joints/*params_.joints*/)
  {
    command_interface_types_.push_back(joint + "/" + /*params_.*/interface_name);
  }

  return controller_interface::CallbackReturn::SUCCESS;
}

bool ForwardCommandController::get_shared_parameters()
{
  if (!SharedParamsInterface::initialize()) {
   RCLCPP_ERROR(get_node()->get_logger(), "Failed to initialize shared memory interface");
    return false;
  }

  try {
    auto params = SharedParamsInterface::getNodeParams("shared_parameters");
    if (params.empty() || (params.find("error_code") != params.end() && params["error_code"].getValue<int>() > 0)) {
      RCLCPP_ERROR(get_node()->get_logger(), "Error get shared parameters");
      return false;
    } 
    
    std::regex pattern_joints(R"(^controllers/([^/]+)/joints$)");
    std::regex pattern_interface(R"(^controllers/([^/]+)/interface_name$)");
    std::smatch match;
    params = SharedParamsInterface::getNodeParams("base");
    std::string controller_name = get_node()->get_name();
    for (const auto& [name, value] : params) {
      if (name.find(controller_name) == std::string::npos)
        continue;

      if (std::regex_match(name, match, pattern_joints)) {
        std::string joints_name = match[0];
        RCLCPP_INFO_STREAM(get_node()->get_logger(), "get joints name:" << joints_name);
        get_node()->declare_parameter("joints", value.getValue<std::string>());
      } else if (std::regex_match(name, match, pattern_interface)) {
        std::string interface_name = match[0];
        RCLCPP_INFO_STREAM(get_node()->get_logger(), "get interface name:" << interface_name);
        get_node()->declare_parameter("interface_name", value.getValue<std::string>());
      }
    }
  } catch (const std::exception& e) {
    RCLCPP_ERROR(get_node()->get_logger(), "ForwardCommandController Error reading parameters: %s", e.what());
    return false;
  }

  return true;
}

std::vector<std::string> 
ForwardCommandController::parse_names(const std::string& input) {
  std::vector<std::string> result;
  std::string current;
  
  for (char c : input) {
    if (c == ',' || c == ' ') {
      if (!current.empty()) {       
        result.push_back(current);
        current.clear();
      }
    } else {
      current += c; 
    }
  }
  
  if (!current.empty()) {
    result.push_back(current);
  }
  
  return result;
}

}  // namespace forward_command_controller

#include "pluginlib/class_list_macros.hpp"

PLUGINLIB_EXPORT_CLASS(
  forward_command_controller::ForwardCommandController, controller_interface::ControllerInterface)
